#include "PhysicCollisionResolution.h"

PhysicCollisionResolution::PhysicCollisionResolution(){};
PhysicCollisionResolution::~PhysicCollisionResolution(){};

void PhysicCollisionResolution::Update(PhysicRigidBody *a_RigidBody, float a_timePassed){
	//Response(a_RigidBody);
	Integrate(a_RigidBody, a_timePassed);
};

void PhysicCollisionResolution::Integrate(PhysicRigidBody *a_RigidBody, float a_timePassed){
	a_RigidBody->m_PhysicObject->m_position.add(
		a_RigidBody->m_PhysicObject->m_velocity.product(a_timePassed));
};

void PhysicCollisionResolution::Response(PhysicRigidBody *a_RigidBody_1){
	int t_size = a_RigidBody_1->m_PhysicRigidBodyContactList.Get_Size();
	for(int i = 0; i < t_size; i++){
		PhysicRigidBody *t_RigidBody_2 = (a_RigidBody_1->m_PhysicRigidBodyContactList.Get_Data(i));
		Penetration(a_RigidBody_1, t_RigidBody_2);
		t_RigidBody_2 = 0;
	}
};

void PhysicCollisionResolution::Penetration(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	switch(a_RigidBody_1->m_PhysicObject->m_physic_object_type){
	case PHYSIC_OBJECT_TYPE_CIRCLE:
		switch(a_RigidBody_2->m_PhysicObject->m_physic_object_type){
		case PHYSIC_OBJECT_TYPE_CIRCLE:
			Penetration_Circle_Circle(a_RigidBody_1, a_RigidBody_2);
			break;
		case PHYSIC_OBJECT_TYPE_SQUARE:
			Penetration_Circle_Square(a_RigidBody_1, a_RigidBody_2);
			break;
		}
		break;
	case PHYSIC_OBJECT_TYPE_SQUARE:
		switch(a_RigidBody_2->m_PhysicObject->m_physic_object_type){
		case PHYSIC_OBJECT_TYPE_CIRCLE:
			Penetration_Square_Circle(a_RigidBody_1, a_RigidBody_2);
			break;
		case PHYSIC_OBJECT_TYPE_SQUARE:
			Penetration_Square_Square(a_RigidBody_1, a_RigidBody_2);
			break;
		}
		break;
	}
};

void PhysicCollisionResolution::Penetration_Circle_Circle(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_radius_1 = a_RigidBody_1->m_PhysicObject->m_radius;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_radius_2 = a_RigidBody_2->m_PhysicObject->m_radius;

	Vector2D tempD = t_position_1.minus(t_position_2);
	float tempMag = tempD.magnitude();
	tempD.normalize();
	float depth = (t_radius_1 + t_radius_2) - tempMag;
	a_RigidBody_1->m_PhysicObject->m_position.set(t_position_1.x+tempD.x, t_position_1.y+tempD.y);
};

void PhysicCollisionResolution::Penetration_Square_Square(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_width_1 = a_RigidBody_1->m_PhysicObject->m_width;
	float t_height_1 = a_RigidBody_1->m_PhysicObject->m_height;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_width_2 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_2 = a_RigidBody_2->m_PhysicObject->m_height;

	float overlapX = ((t_width_1*0.5f) + (t_width_2*0.5f)) 
		- (fabs(t_position_1.x - t_position_2.x));

	float overlapY = ((t_height_1*0.5f) + (t_height_2*0.5f)) 
		- (fabs(t_position_1.y - t_position_2.y));

	Vector2D t_newPosition = a_RigidBody_1->m_PhysicObject->m_position;

	Vector2D tempD = t_position_1.minus(t_position_2);
	tempD.normalize();

	a_RigidBody_1->m_PhysicObject->m_position.set(t_position_1.x+tempD.x, t_position_1.y+tempD.y);
};

void PhysicCollisionResolution::Penetration_Circle_Square(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_radius_1 = a_RigidBody_1->m_PhysicObject->m_radius;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_width_2 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_2 = a_RigidBody_2->m_PhysicObject->m_height;

	Vector2D tempD = t_position_1.minus(t_position_2);
	tempD.normalize();

	a_RigidBody_1->m_PhysicObject->m_position.set(t_position_1.x+tempD.x, t_position_1.y+tempD.y);
};

void PhysicCollisionResolution::Penetration_Square_Circle(PhysicRigidBody *a_RigidBody_1, PhysicRigidBody *a_RigidBody_2){
	Vector2D t_position_1 = a_RigidBody_1->m_PhysicObject->m_position;
	float t_width_1 = a_RigidBody_2->m_PhysicObject->m_width;
	float t_height_1 = a_RigidBody_2->m_PhysicObject->m_height;
	Vector2D t_position_2 = a_RigidBody_2->m_PhysicObject->m_position;
	float t_radius_2 = a_RigidBody_1->m_PhysicObject->m_radius;

	Vector2D tempD = t_position_1.minus(t_position_2);
	tempD.normalize();

	a_RigidBody_1->m_PhysicObject->m_position.set(t_position_1.x+tempD.x, t_position_1.y+tempD.y);
};